unit oBehaviorTypes;

interface

uses Classes, Math, oMathematic, oVector, uOSPRand, oPedActor,
     oLevel, oSimulation, Generics.Collections, oPhysicalEntity,
     oCoordinateSystem, oBehavior, uOSPABA, oPedestrian;

type


  TSeek = class(TBehavior)
  private
  public
    constructor Create;
    constructor CreateCopy(paSeek: TSeek);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
  end;

  TFlee = class(TBehavior)
  private
  public
    constructor Create;
    constructor CreateCopy(paFlee: TFlee);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
  end;

  TArrival = class(TBehavior)
  private
    aDecelerateDistance: TFloat;
  public
    constructor Create(paDecelerateDistance: TFloat);
    constructor CreateCopy(paArrival: TArrival);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
    property DecelerateDistance: TFloat read aDecelerateDistance write aDecelerateDistance;
  end;

  TSeparation = class(TBehavior)
  private
    aRadius: TFloat;
    function GetNearPedestrians(paPed: TActor; paRadius: TFloat; paAhead: Boolean; paCosinus: TFloat): TList<TPedestrian>;
    function GetNearPedestrian(paPed: TActor; paRadius: TFloat; paAhead: Boolean; paCosinus: TFloat): TPedestrian;
    function IsAhead(paPed: TPedestrian; paTarget: TVector; paCosinus: TFloat): Boolean;
    function GetIntersection(paPed: TActor; paPosition: TVector; paRadius: TFloat; paMultiplikator: TFloat): TVector;
  public
    constructor Create(paRadius: TFloat);
    constructor CreateCopy(paSeparation: TSeparation);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
    property Radius: TFloat read aRadius write aRadius;
  end;

{  TWander = class(TBehavior)
  private
    aRadius: TFloat;
  public
    constructor Create(paRadius: TFloat);
    constructor CreateCopy(paWander: TWander);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
    property Radius: TFloat read aRadius write aRadius;
  end;

  TAlignment = class(TBehavior)
  private
    aRadius: TFloat;
  public
    constructor Create(paRadius: TFloat);
    constructor CreateCopy(paAlignment: TAlignment);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
    property Radius: TFloat read aRadius write aRadius;
  end;

  TCohesion = class(TBehavior)
  private
    aRadius: TFloat;
  public
    constructor Create(paRadius: TFloat);
    constructor CreateCopy(paCohesion: TCohesion);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
    property Radius: TFloat read aRadius write aRadius;
  end;   }

  TObstacleAvoidance = class(TBehavior)
  private
    aRadius: TFloat;
  public
    constructor Create(paRadius: TFloat);
    constructor CreateCopy(paObstacleAvoidance: TObstacleAvoidance);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
    function GetRepulsive(paObstacle: TPhysicalEntity; paPedestrian: TPedActor; var Distance: TFloat): TVector;
    function GetIntersection(paObstacle: TPhysicalEntity; paPosition, paVector: TVector; var Distance, Angle: TFloat; var paEdge: array of TVector): Boolean;
    property Radius: TFloat read aRadius write aRadius;
  end;
 {
  TPathFollow = class(TBehavior)
  private
  public
    constructor Create;
    constructor CreateCopy(paPathFollow: TPathFollow);
    destructor Destroy;override;
    function Calculate(paPedestrian: TActor): TVector;override;
  end;     }

implementation

uses oPolygon;


constructor TSeek.Create;
begin
    inherited Create;
end;

constructor TSeek.CreateCopy(paSeek: TSeek);
begin
    inherited CreateCopy(paSeek);
end;

destructor TSeek.Destroy;
begin
    inherited Destroy;
end;

function TSeek.Calculate(paPedestrian: TActor): TVector;
var DesiredVelocity, Dest, Pos: TVector;
begin
    Dest := TVector.Create((paPedestrian as TPedActor).Target.Position);
    Pos := TVector.Create((paPedestrian as TPedActor).Pedestrian.Position);
    DesiredVelocity := Dest.Subtract(Pos);
    DesiredVelocity.TruncateSelf((paPedestrian as TPedActor).Pedestrian.MaxVelocity);
    DesiredVelocity.SubtractSelf((paPedestrian as TPedActor).Pedestrian.Velocity);
    Result := DesiredVelocity;
end;

constructor TFlee.Create;
begin
    inherited Create;
end;

constructor TFlee.CreateCopy(paFlee: TFlee);
begin
    inherited CreateCopy(paFlee);
end;

destructor TFlee.Destroy;
begin
    inherited Destroy;
end;

function TFlee.Calculate(paPedestrian: TActor): TVector;
var DesiredVelocity, Dest, Pos: TVector;
begin
    Dest := TVector.Create((paPedestrian as TPedActor).Target.Position);
    Pos := TVector.Create((paPedestrian as TPedActor).Pedestrian.Position);
    DesiredVelocity := Dest.Subtract(Pos);
    DesiredVelocity.TruncateSelf((paPedestrian as TPedActor).Pedestrian.MaxVelocity);
    Result := (paPedestrian as TPedActor).Pedestrian.Velocity.Subtract(DesiredVelocity);
    DesiredVelocity.Free;
end;

constructor TArrival.Create(paDecelerateDistance: TFloat);
begin
    inherited Create;
    aDecelerateDistance := paDecelerateDistance;
end;

constructor TArrival.CreateCopy(paArrival: TArrival);
begin
    inherited CreateCopy(paArrival);
    aDecelerateDistance := paArrival.aDecelerateDistance;
end;

destructor TArrival.Destroy;
begin
    inherited Destroy;
end;

function TArrival.Calculate(paPedestrian: TActor): TVector;
var DesiredVelocity, Dest, Pos: TVector;
    Distance, Speed: TFloat;
begin
    Dest := TVector.Create((paPedestrian as TPedActor).Target.Position);
    Pos := TVector.Create((paPedestrian as TPedActor).Pedestrian.Position);
    DesiredVelocity := Dest.Subtract(Pos);
    Distance := DesiredVelocity.Length;
    Speed := Min(Distance / aDecelerateDistance, (paPedestrian as TPedActor).Pedestrian.MaxVelocity);
    DesiredVelocity.MultiplySelf(Speed / Distance);
    DesiredVelocity.SubtractSelf((paPedestrian as TPedActor).Pedestrian.Velocity);
    DesiredVelocity.TruncateSelf((paPedestrian as TPedActor).Pedestrian.MaxVelocity);
    Result := DesiredVelocity;
end;

constructor TSeparation.Create(paRadius: TFloat);
begin
    inherited Create;
    aRadius := paRadius;
end;

constructor TSeparation.CreateCopy(paSeparation: TSeparation);
begin
    inherited CreateCopy(paSeparation);
    aRadius := paSeparation.Radius;
end;

destructor TSeparation.Destroy;
begin
    inherited Destroy;
end;

function TSeparation.IsAhead(paPed: TPedestrian; paTarget: TVector; paCosinus: TFloat): Boolean;
var
  LocalPosition: TVector;
begin
  LocalPosition := paPed.LocalSpace.LocalizePosition(paTarget);
  if (LocalPosition.X > 0) then Result := true else Result:= false;
  LocalPosition.Free;
end;

function TSeparation.GetNearPedestrians(paPed: TActor; paRadius: Double; paAhead: Boolean; paCosinus: Double): TList<TPedestrian>;
var
  NearPedestriansList: TList<TPedestrian>;
  PedestriansList: TList<TPedestrian>;
  Distance: TFloat;
  I, levelNumber: Integer;
begin
  NearPedestriansList := TList<TPedestrian>.Create;
  levelNumber := (paPed as TPedActor).Target.Level;
  PedestriansList := ((paPed as TPedActor).MySim as TPedSimulation).World.getLevel(levelNumber).getPedestrians((paPed as TPedActor).Pedestrian);
    if PedestriansList <> nil then begin
        for I := 0 to PedestriansList.Count - 1 do begin
            Distance := (PedestriansList[I] as TPedestrian).SSPosition.Distance((paPed as TPedActor).Pedestrian.SSPosition);// - TPedestrian(aPedestriansList[I]).aWidth - aWidth;
            if ((PedestriansList[I] as TPedestrian) <> (paPed as TPedActor).Pedestrian) and (Distance < paRadius) then begin
                if not paAhead then begin
                    NearPedestriansList.Add(PedestriansList[I])
                end
                else if IsAhead((paPed as TPedActor).Pedestrian ,(PedestriansList[I] as TPedestrian).SSPosition, paCosinus) then begin
                    NearPedestriansList.Add(PedestriansList[I]);
                end;
            end;
        end;
    end;
    Result := NearPedestriansList;
end;

function TSeparation.GetNearPedestrian(paPed: TActor; paRadius: Double; paAhead: Boolean; paCosinus: Double): TPedestrian;
var
  I, levelNumber: Integer;
  MinimalDistance, Distance: TFloat;
  Pedestrian: TPedestrian;
  PedestriansList: TList<TPedestrian>;
begin
  Pedestrian := nil;
  levelNumber := (paPed as TPedActor).Target.Level;
  PedestriansList := ((paPed as TPedActor).MySim as TPedSimulation).World.getLevel(levelNumber).getPedestrians((paPed as TPedActor).Pedestrian);
  if PedestriansList <> nil then
    begin
      MinimalDistance := MaxFloat;
      for I := 0 to PedestriansList.Count - 1 do
        begin
          Distance := (PedestriansList[I] as TPedestrian).SSPosition.Distance((paPed as TPedActor).Pedestrian.SSPosition);// - TPedestrian(aPedestriansList[I]).aWidth - aWidth;
          if ((PedestriansList[I] as TPedestrian) <> (paPed as TPedActor).Pedestrian) and (Distance < paRadius) and (Distance < MinimalDistance) then
            begin
              if not paAhead then
                begin
                  Pedestrian := PedestriansList[I] as TPedestrian;
                  MinimalDistance := Distance;
                end
              else if IsAhead((paPed as TPedActor).Pedestrian, (PedestriansList[I] as TPedestrian).SSPosition, paCosinus) then
                begin
                  Pedestrian := PedestriansList[I] as TPedestrian;
                  MinimalDistance := Distance;
                end;
            end;
        end;
    end;
  Result := Pedestrian;
end;

function TSeparation.GetIntersection(paPed: TActor; paPosition: TVector3f; paRadius: Double; paMultiplikator: Double): TVector;
var
  LocalPosition, Intrusion, Vector: TVector;
begin
  Intrusion := TVector.Create;
  LocalPosition := (paPed as TPedActor).Pedestrian.LocalSpace.LocalizePosition(paPosition);
  if (LocalPosition.Y >= 0) and (LocalPosition.Y - paRadius * paMultiplikator < (paPed as TPedActor).Pedestrian.Diameter) then
    begin
      Intrusion.SetXYZ(0.0, LocalPosition.Y - paRadius * paMultiplikator, 0.0);
      Vector := TVector.Create(0.0, (paPed as TPedActor).Pedestrian.Diameter, 0.0);
      Intrusion.SubtractSelf(Vector);
      Vector.Free;
    end
  else if (LocalPosition.Y < 0) and (LocalPosition.Y + paRadius * paMultiplikator > -(paPed as TPedActor).Pedestrian.Diameter) then
    begin
      Intrusion.SetXYZ(0.0, LocalPosition.Y + paRadius * paMultiplikator, 0.0);
      Vector := TVector.Create(0.0, -(paPed as TPedActor).Pedestrian.Diameter, 0.0);
      Intrusion.SubtractSelf(Vector);
      Vector.Free;
    end;
  LocalPosition.Free;
  Result := (paPed as TPedActor).Pedestrian.LocalSpace.GlobalizeDirection(Intrusion);
  Intrusion.Free;
end;


function TSeparation.Calculate(paPedestrian: TActor): TVector;
var
    DesiredVelocity, NegateVector, Intrusion, LocalPosition: TVector;
    NearPedestriansList: TList<TPedestrian>;
    NearPedestrian: TPedestrian;
    I, levelNumber: Integer;
begin
    DesiredVelocity := TVector.Create(0.0, 0.0, 0.0);
    NearPedestriansList := GetNearPedestrians(paPedestrian, aRadius / 2.0, true, 0.5);
    if NearPedestriansList.Count <> 0 then begin
        for I := 0 to NearPedestriansList.Count - 1 do begin
            NegateVector := (paPedestrian as TPedActor).Pedestrian.SSPosition.Subtract((NearPedestriansList[I] as TPedestrian).SSPosition);
            DesiredVelocity.AddSelf(NegateVector.DivideSelf((paPedestrian as TPedActor).Pedestrian.SSPosition.Distance(TPedestrian(NearPedestriansList[I]).SSPosition)));
            NegateVector.Free;
        end;
    end;
    NearPedestriansList.Free;
    NearPedestrian := GetNearPedestrian(paPedestrian, aRadius, false, 0.5);
    if NearPedestrian <> nil then begin
        Intrusion := GetIntersection(paPedestrian, NearPedestrian.SSPosition, NearPedestrian.Diameter, 1.5);
        if Intrusion.Length <> 0 then begin
            Intrusion.DivideSelf((paPedestrian as TPedActor).Pedestrian.SSPosition.Distance(NearPedestrian.SSPosition));
            DesiredVelocity.AddSelf(Intrusion);
        end;
 {
        LocalPosition := (paPedestrian as TPedActor).Pedestrian.LocalSpace.LocalizePosition(NearPedestrian.SSPosition);
        if (LocalPosition.Y < 0) and (LocalPosition.X <= (paPedestrian as TPedActor).Pedestrian.Diameter / 2.0) and (LocalPosition.X >= -(paPedestrian as TPedActor).Pedestrian.Diameter / 2.0) then
                (paPedestrian as TPedActor).Pedestrian.MaxAcceleration := 0.0
        else (paPedestrian as TPedActor).Pedestrian.MaxAcceleration := 1.0;
        LocalPosition.Free;
  }
        Intrusion.Free;
    end
    else (paPedestrian as TPedActor).Pedestrian.MaxAcceleration := 1.0;
    Result := DesiredVelocity;
end;
           {
constructor TWander.Create(paRadius: TFloat);
begin
    inherited Create;
    aRadius := paRadius;
end;

constructor TWander.CreateCopy(paWander: TWander);
begin
    inherited CreateCopy(paWander);
    aRadius := paWander.Radius;
end;

destructor TWander.Destroy;
begin
    inherited Destroy;
end;

function TWander.Calculate(paPedestrian: TPedActor): TVector;
var Pedestrian: TPedestrian;
    DesiredVelocity, LocalVector: TVector;
    Randomizer: tRndm;
begin
    Pedestrian := TPedestrian(paPedestrian);
    Randomizer := tRndm.Create(true);
    LocalVector := TVector.Create(Pedestrian.MaxSpeed, Randomizer.Rand * aRadius, 0.0);
    if Randomizer.Rand < 0.5 then LocalVector.Y := -LocalVector.Y;
    LocalVector.ScaleSelf(Pedestrian.MaxSpeed);
    DesiredVelocity := Pedestrian.LocalSpace.GlobalizeDirection(LocalVector);
    DesiredVelocity.AddSelf(Pedestrian.Velocity);
    DesiredVelocity.ScaleSelf(Pedestrian.MaxSpeed);
    LocalVector.Free;
    Randomizer.Free;
    Result := DesiredVelocity;
end;

constructor TAlignment.Create(paRadius: TFloat);
begin
    inherited Create;
    aRadius := paRadius;
end;

constructor TAlignment.CreateCopy(paAlignment: TAlignment);
begin
    inherited CreateCopy(paAlignment);
    aRadius := paAlignment.Radius;
end;

destructor TAlignment.Destroy;
begin
    inherited Destroy;
end;

function TAlignment.Calculate(paPedestrian: TPedActor): TVector;
var Pedestrian: TPedestrian;
    DesiredVelocity, Direction, LocalDirection: TVector;
    NearPedestriansList: TPedestriansList;
    I: Integer;
begin
    Pedestrian := TPedestrian(paPedestrian);
    DesiredVelocity := TVector.Create(0.0, 0.0, 0.0);
    NearPedestriansList := Pedestrian.GetNearPedestrians(aRadius, true, 0.5);
    if NearPedestriansList.Count <> 0 then begin
        for I := 0 to NearPedestriansList.Count - 1 do begin
            Direction := TPedestrian(NearPedestriansList[I]).Destination.Subtract(TPedestrian(NearPedestriansList[I]).Position);
            LocalDirection := Pedestrian.LocalSpace.LocalizeDirection(Direction);
            Direction.Free;
            if LocalDirection.X > 0 then
                DesiredVelocity.AddSelf(TPedestrian(NearPedestriansList[I]).Velocity);
            LocalDirection.Free;
        end;
        DesiredVelocity.DivideSelf(NearPedestriansList.Count);
        DesiredVelocity.SubtractSelf(Pedestrian.LocalSpace.Forward);
    end;
    NearPedestriansList.Free;
    Result := DesiredVelocity;
end;

constructor TCohesion.Create(paRadius: TFloat);
begin
    inherited Create;
    aRadius := paRadius;
end;

constructor TCohesion.CreateCopy(paCohesion: TCohesion);
begin
    inherited CreateCopy(paCohesion);
    aRadius := paCohesion.Radius;
end;

destructor TCohesion.Destroy;
begin
    inherited Destroy;
end;

function TCohesion.Calculate(paPedestrian: TPedActor): TVector;
var Pedestrian: TPedestrian;
    DesiredVelocity, Direction, LocalDirection: TVector;
    NearPedestriansList: TPedestriansList;
    I: Integer;
begin
    Pedestrian := TPedestrian(paPedestrian);
    DesiredVelocity := TVector.Create(0.0, 0.0, 0.0);
    NearPedestriansList := Pedestrian.GetNearPedestrians(aRadius, true, 0.5);
    if NearPedestriansList.Count <> 0 then begin
        for I := 0 to NearPedestriansList.Count - 1 do begin
            Direction := TPedestrian(NearPedestriansList[I]).Destination.Subtract(TPedestrian(NearPedestriansList[I]).Position);
            LocalDirection := Pedestrian.LocalSpace.LocalizeDirection(Direction);
            Direction.Free;
            if LocalDirection.X > 0 then
                DesiredVelocity.AddSelf(TPedestrian(NearPedestriansList[I]).Position);
            LocalDirection.Free;
        end;
        DesiredVelocity.DivideSelf(NearPedestriansList.Count);
        DesiredVelocity.SubtractSelf(Pedestrian.Position);
        DesiredVelocity.NormalizeSelf;
    end;
    NearPedestriansList.Free;
    Result := DesiredVelocity;
end;
        }
constructor TObstacleAvoidance.Create(paRadius: TFloat);
begin
    inherited Create;
    aRadius := paRadius;
end;

constructor TObstacleAvoidance.CreateCopy(paObstacleAvoidance: TObstacleAvoidance);
begin
    inherited CreateCopy(paObstacleAvoidance);
    aRadius := paObstacleAvoidance.Radius;
end;

destructor TObstacleAvoidance.Destroy;
begin
    inherited Destroy;
end;

function TObstacleAvoidance.Calculate(paPedestrian: TActor): TVector;
var
    NearObstacleList: TList<TPhysicalEntity>;
    Repulsive, Temp: TVector;
    Distance, MinimumDistance: TFloat;
    I, levelNumber: Integer;
begin
    Repulsive := TVector.Create;
    MinimumDistance := MaxFloat;
    levelNumber := (paPedestrian as TPedActor).Target.Level;
    NearObstacleList := ((paPedestrian as TPedActor).MySim as TPedSimulation).World.getLevel(levelNumber).getPhysicalEntities((paPedestrian as TPedActor).Pedestrian.Position,aRadius); //Pedestrian.GetNearObstacles(aRadius, false, 0.5);
    if NearObstacleList.Count <> 0 then begin
        for I := 0 to NearObstacleList.Count - 1 do begin
            Temp := GetRepulsive((NearObstacleList[I] as TPhysicalEntity),(paPedestrian as TPedActor), Distance);
            if Distance < MinimumDistance then begin
                MinimumDistance := Distance;
                Repulsive.MakeCopy(Temp);
            end;
            Temp.Free;
        end;
    end;
    NearObstacleList.Free;
    Result := Repulsive;
end;

function TObstacleAvoidance.GetIntersection(paObstacle: TPhysicalEntity; paPosition, paVector: TVector; var Distance, Angle: TFloat; var paEdge: array of TVector): Boolean;
var LocalSpace: TCoordinateSystem;
    Point1, Point2, LocalPoint1, LocalPoint2, NearestPoint1, NearestPoint2: TVector;
    Intersection, MinimalIntersection, Length: TFloat;
    Intersect: Boolean;
    I: Integer;
    aEdge: TVector;
begin
    Length := paVector.Length;
    Distance := MaxFloat;
    MinimalIntersection := MaxFloat;
    Distance := MaxFloat;
    Intersect := false;
    NearestPoint1 := TVector.Create;
    NearestPoint2 := TVector.Create;
    for I := 0 to (paObstacle as TPolygon).Points.Count - 1 do begin
        if I = (paObstacle as TPolygon).Points.Count - 1 then begin
            Point1 := TVector((paObstacle as TPolygon).Points[I]);
            Point2 := TVector((paObstacle as TPolygon).Points[0]);
           { //SKUSKA
            Point1.X := Point1.X - paPosition.X;
            Point2.X := Point2.X - paPosition.X;
            Point1.Y := Point1.Y - paPosition.Y;
            Point2.Y := Point2.Y - paPosition.Y; }
        end
        else begin
            Point1 := TVector((paObstacle as TPolygon).Points[I]);
            Point2 := TVector((paObstacle as TPolygon).Points[I + 1]);
            {//SKUSKA
            Point1.X := Point1.X - paPosition.X;
            Point2.X := Point2.X - paPosition.X;
            Point1.Y := Point1.Y - paPosition.Y;
            Point2.Y := Point2.Y - paPosition.Y; }
        end;
        LocalSpace := TCoordinateSystem.Create;
        LocalSpace.Setup(paPosition, paVector);
        LocalPoint1 := LocalSpace.LocalizePosition(Point1);
        LocalPoint2 := LocalSpace.LocalizePosition(Point2);
        if ((LocalPoint1.Y < 0) and (LocalPoint2.Y < 0)) or ((LocalPoint1.Y > 0) and (LocalPoint2.Y > 0)) then begin
            LocalPoint1.Free;
            LocalPoint2.Free;
            LocalSpace.Free;
            continue;
        end;
        Intersection := (-LocalPoint1.Y * (LocalPoint2.X - LocalPoint1.X)) / (LocalPoint2.Y - LocalPoint1.Y) + LocalPoint1.X;
        if (Intersection > 0) and (Intersection < Length) and (Intersection < MinimalIntersection) then begin
            Intersect := true;
            MinimalIntersection := Intersection;
            NearestPoint1.MakeCopy(LocalPoint1);
            NearestPoint2.MakeCopy(LocalPoint2);
            if (paEdge[0] <> nil) and (paEdge[1] <> nil) then begin
                paEdge[0].MakeCopy(Point1);
                paEdge[1].MakeCopy(Point2);
            end;
        end;
        LocalPoint1.Free;
        LocalPoint2.Free;
        LocalSpace.Free;
    end;
    if Intersect then begin
        Distance := MinimalIntersection;
        Angle := RadToDeg(ArcTan2(-NearestPoint1.Y, (MinimalIntersection - NearestPoint1.X)));
    end;
    NearestPoint1.Free;
    NearestPoint2.Free;
    Result := Intersect;
end;


function TObstacleAvoidance.GetRepulsive(paObstacle: TPhysicalEntity; paPedestrian: TPedActor; var Distance: TFloat): TVector;
var
    RepulsiveForward, RepulsiveRight, RepulsiveLeft, TestVector, Right, Left: TVector;
    NearestEdge: array [0..1] of TVector;
    TempDistance, Angle, ScaleFactor: TFloat;
begin

    NearestEdge[0] := TVector.Create;
    NearestEdge[1] := TVector.Create;

    Distance := MaxFloat;
    RepulsiveForward := TVector.Create;
    RepulsiveRight := TVector.Create;
    RepulsiveLeft := TVector.Create;

    TestVector := TVector.CreateCopy(paPedestrian.Pedestrian.LocalSpace.Forward);
   // TestVector := TVector.CreateCopy(paPedestrian.Pedestrian.SSPosition);  //SKUSKA, inak plati riadok predtym
    //TestVector.X := paPedestrian.Pedestrian.SSPosition.X + 1.0;

    TestVector.ScaleSelf(paPedestrian.Pedestrian.MaxVelocity); // .Velocity.Length * 10);
    if GetIntersection(paObstacle, paPedestrian.Pedestrian.SSPosition, TestVector, TempDistance, Angle, NearestEdge) then begin
        if TempDistance < Distance then Distance := TempDistance;
        ScaleFactor := Abs(Sin(Angle) * (1.0 / TempDistance));
        RepulsiveForward.X := (NearestEdge[1].Y - NearestEdge[0].Y);
        RepulsiveForward.Y := -(NearestEdge[1].X - NearestEdge[0].X);
        RepulsiveForward.ScaleSelf(ScaleFactor);
    end;
    TestVector.Free;

    Right := TVector.Create(1.0, 1.0, 0.0);
    TestVector := paPedestrian.Pedestrian.LocalSpace.GlobalizeDirection(Right);
    //TestVector.ScaleSelf(paPedestrian.Pedestrian.Velocity.Length * 10);
    TestVector.ScaleSelf(paPedestrian.Pedestrian.MaxVelocity);
    if GetIntersection(paObstacle, paPedestrian.Pedestrian.SSPosition, TestVector, TempDistance, Angle, NearestEdge) then begin
        if TempDistance < Distance then Distance := TempDistance;
        ScaleFactor := Abs(Sin(Angle) * (1.0 / TempDistance));
        RepulsiveRight.X := (NearestEdge[1].Y - NearestEdge[0].Y);
        RepulsiveRight.Y := -(NearestEdge[1].X - NearestEdge[0].X);
        RepulsiveRight.ScaleSelf(ScaleFactor);
    end;
    TestVector.Free;
    Right.Free;

    Left := TVector.Create(1.0, -1.0, 0.0);
    TestVector := paPedestrian.Pedestrian.LocalSpace.GlobalizeDirection(Left);
    //TestVector.ScaleSelf(paPedestrian.Pedestrian.Velocity.Length * 10);
    TestVector.ScaleSelf(paPedestrian.Pedestrian.MaxVelocity);
    if GetIntersection(paObstacle, paPedestrian.Pedestrian.SSPosition, TestVector, TempDistance, Angle, NearestEdge) then begin
        if TempDistance < Distance then Distance := TempDistance;
        ScaleFactor := Abs(Sin(Angle) * (1.0 / Distance));
        RepulsiveLeft.X := (NearestEdge[1].Y - NearestEdge[0].Y);
        RepulsiveLeft.Y := -(NearestEdge[1].X - NearestEdge[0].X);
        RepulsiveLeft.ScaleSelf(ScaleFactor);
    end;
    TestVector.Free;
    Left.Free;

    NearestEdge[0].Free;
    NearestEdge[1].Free;
    Result := RepulsiveForward.AddSelf(RepulsiveRight).AddSelf(RepulsiveLeft);
    RepulsiveRight.Free;
    RepulsiveLeft.Free;
end;
{
constructor TPathFollow.Create;
begin
    inherited Create;
end;

constructor TPathFollow.CreateCopy(paPathFollow: TPathFollow);
begin
    inherited Create;
end;

destructor TPathFollow.Destroy;
begin
    inherited Destroy;
end;

function TPathFollow.Calculate(paPedestrian: TPedActor): TVector;
var Pedestrian: TPedestrian;
    DesiredVelocity: TVector;
begin
    Pedestrian := TPedestrian(paPedestrian);
    if (Pedestrian.Path <> nil) and (Pedestrian.Path <> nil) and (Pedestrian.Path.Count <> 0) then begin
        if Pedestrian.PathCounter = -1 then begin
            if Pedestrian.PathReverse then Pedestrian.PathCounter := Pedestrian.Path.Count - 1
            else Pedestrian.PathCounter := 0;
        end;
        if Pedestrian.Position.IsEqual(TVector(Pedestrian.Path[Pedestrian.PathCounter]), TPathPoint(Pedestrian.Path[Pedestrian.PathCounter]).Radius) then begin
            if (Pedestrian.PathReverse) and (Pedestrian.PathCounter > 0) then
                Pedestrian.PathCounter := Pedestrian.PathCounter - 1
            else if (not Pedestrian.PathReverse) and (Pedestrian.PathCounter < Pedestrian.Path.Count - 1)
                then Pedestrian.PathCounter := Pedestrian.PathCounter + 1;
        end;
        DesiredVelocity := TVector(Pedestrian.Path[Pedestrian.PathCounter]).Subtract(Pedestrian.Position);
        DesiredVelocity.TruncateSelf(Pedestrian.MaxSpeed);
        DesiredVelocity.SubtractSelf(Pedestrian.Velocity);
        Result := DesiredVelocity;
    end
    else Result := TVector.Create;
end;     }

end.
